/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "PoseMCL.h"

CPoseMCL::CPoseMCL(){
	initializeVariables(2.0, 0.995,45.0*M_PI/180.0,0.95);
}
CPoseMCL::CPoseMCL(float pDistTh, float pDist2thLik, float pHeadErrorTh, float pHeadCutTh ){
	initializeVariables(pDistTh, pDist2thLik, pHeadErrorTh, pHeadCutTh);
}


/**
* Initializes internal variables
* @param pDistTh The largest acceptaple distance error(m) (more than this will give zero likelihood)
* @param pDist2thLik If the likelihood is more than threshold the likelihood is cutted to this value
* @param pHeadErrorTh The largest acceptaple heading error (rad)
* @param pHeadCutTh If the likelihood is more than threshold the likelihood is cutted to this value
* 
* Below is an octave script for testing the parameters
pDistTh = 2;
pDist2thLik=0.995;
dist = 0:0.01:pDistTh;
dist2=dist.^2;
cost=(pDistTh*pDistTh-dist2)/(pDistTh*pDistTh);
cost(cost>pDist2thLik)=pDist2thLik;
plot(dist,cost,'.');
grid on;

pHeadErrorTh = 45*pi/180;
pHeadCutTh = 0.95;
angle = 0:0.01:pHeadErrorTh;
cost = (pHeadErrorTh-angle)/pHeadErrorTh;
cost(cost>pHeadCutTh)=pHeadCutTh;
plot(angle*180/pi,cost);


*/
void CPoseMCL::initializeVariables(float pDistTh, float pDist2thLik, float pHeadErrorTh, float pHeadCutTh ){

	paramDist2th=pDistTh*pDistTh;
	paramDist2thLik = pDist2thLik;
	paramHeadErrorTh = pHeadErrorTh;
	paramHeadCutTh = pHeadCutTh;
}

/**
* Initializes the filter - uses normal random method for calculating the initial distribution
* Loads the map from line file, reserves memory for particles, sets the initial distribution
* @p num_of_particles The number of particles in the filter
* @p init_pose the initial pose
* @p initVariance the variance for the initial quess
*/
void CPoseMCL::initialize(int num_of_particles, mcl::pose initPose,	mcl::pose initVariance){
	filt.initializeNormalRandom(initPose, initVariance,num_of_particles);	
}


void CPoseMCL::predict(mcl::pose dP,mcl::pose variance){
		filt.predict(dP,variance); ///<Predict
}

/**
* Run the MCL step
* @p dP the relative movement between steps
* @p variance prediction error 
*/
void CPoseMCL::update(mcl::pose P){
	mcl::pose var;
	static int sinceSIR=0; 
	
	calculateLikelihood(P); ///<Calculate likelihoods
	filt.normalize(); 
	
	var = filt.getDistributionVariances(); 
	
	///Calculate the variance of the probabilities
	float varP=0;
	float iN = 1.0 / (float) filt.NumOfParticles;
	float maxP = -1;
	for(int i = 0; i<filt.NumOfParticles;i++){
		varP += (filt.Particles[i].p - iN)*(filt.Particles[i].p - iN);
		if(filt.Particles[i].p > maxP) maxP = filt.Particles[i].p;
	}
	
	///Using now stetson harrison method
	//fprintf(stderr,"%f \n",varP);
	if((varP > .001 || sinceSIR > 1000) && sinceSIR>2){
		fprintf(stderr,"Performing SIR varP=%.6f (%d)\n",varP,sinceSIR);
		filt.SIRUpdate();
		sinceSIR = 0;
	}
	sinceSIR++;
}
/**
* Check if the distribution is valid for given pose
*/
bool CPoseMCL::isValidDistribution(mcl::pose P){
	float rTolerance=0.25 * 0.25;
	float aTolerance=5.0*M_PI/180.0;
	
	int Ninside=0;
	for(int i = 0; i<filt.NumOfParticles;i++){
		float dist2 = pow((filt.Particles[i].x - P.x),2)+pow((filt.Particles[i].y - P.y),2); ///Squared distance error
		float headE = fabs(P.a-filt.Particles[i].a); ///Absolute of heading error
		
		if(headE > M_PI) while(headE>M_PI) headE-=2.0*M_PI;
		headE = fabs(headE);
		
		if( dist2<rTolerance && headE < aTolerance ) Ninside++;
	}

	if(Ninside < 10){
		fprintf(stderr,"Only %d inside given tolerance\n",Ninside);
		return false; 
	}
	return true;
}



/**
* Calculates the likelihood of particle distribution
* @param P pose measurement for the current distribution
**/
void CPoseMCL::calculateLikelihood(mcl::pose P)
{
	int i;		
	filt.Lik = 0;
	filt.outLiers = 0;
	float lik=0;
	float dist2=0;
	float headE=0;
	
	float distLik = 0;
	float headLik = 0;
	
	for(i=0;i<filt.NumOfParticles;i++){
		dist2=pow((filt.Particles[i].x - P.x),2)+pow((filt.Particles[i].y - P.y),2); ///Squared distance error
		headE = fabs(P.a-filt.Particles[i].a); ///Absolute of heading error
		
		if(headE > M_PI) while(headE>M_PI) headE-=2.0*M_PI;
		headE = fabs(headE);
		
		///Computes cutted second order curve as likelihood
		distLik = (paramDist2th - dist2) / paramDist2th;
		if(distLik < 0){
			distLik = 0;
		}else if(distLik > paramDist2thLik){
			distLik=paramDist2thLik;
		}
		///Cutted Linear curve for heading likelihood 
		headLik = (paramHeadErrorTh-headE)/paramHeadErrorTh;
		if(headLik<0){
			headLik = 0;
		}else if(headLik > paramHeadCutTh){
			headLik = paramHeadCutTh;
		}
		
		lik = distLik*headLik;
		filt.Particles[i].lik=lik;
		filt.Lik+=filt.Particles[i].lik;
	}
}